clc; clear; close all;

%% 仿真参数
% 惯性矩
I11 = 1.25; I22 = 1.25; I33 = 2.5; % kg·m^2
J = diag([I11, I22, I33]); % 惯性矩阵

% 控制参数
T_sigma1 = 1; T_sigma2 = 1; T_s = 4; T_lambda = 4;
sigma1 = 5/3; sigma2 = 3/2; sigma3 = 37/39; sigma4 = 39/37;
lambda1 = 1/2; lambda2 = 3/2;
pi_0 = 0.1; eta_d = 0.9; eta_lambda = 0.9;
cd = 84; c_lambda = 3;

% 期望姿态和外部扰动
phi_d = @(t) 0.1 * [sin(t); cos(t); 0]; % 期望姿态 (rad)
phi_d_dot = @(t) 0.1 * [cos(t); -sin(t); 0]; % 期望角速度
phi_d_ddot = @(t) -0.1 * [sin(t); cos(t); 0]; % 期望角加速度
tau_d = @(t) 0.1 * [sin(t); cos(t); sin(2*t)]; % 外部扰动 (N·m)

% 初始条件
phi_0 = [-0.1; 0.2; 0.2]; % 初始姿态 (rad)
phi_dot_0 = [0; 0; 0]; % 初始角速度 (rad/s)
tau_hat_0 = [0; 0; 0]; % 初始干扰估计 (N·m)
gamma_hat_0 = 1; % 初始自适应参数

% 初始状态向量
x0 = [phi_0; phi_dot_0; tau_hat_0; gamma_hat_0];

% 仿真时间
T_sim = 10; % 仿真时间 (s)

%% 使用 ode45 求解
[t, x] = ode45(@(t, x) quadrotorDynamics(t, x, J, phi_d, phi_d_dot, phi_d_ddot, tau_d, ...
                                         sigma2, cd, c_lambda, eta_d, eta_lambda), [0, T_sim], x0);

%% 提取结果
phi = x(:, 1:3)'; % 姿态角
phi_dot = x(:, 4:6)'; % 姿态角速度
tau_hat = x(:, 7:9)'; % 干扰估计
gamma_hat = x(:, 10)'; % 自适应参数

%% 绘图
figure;
subplot(3, 1, 1);
plot(t, phi);
% title('姿态角');
xlabel('时间 (s)');
ylabel('角度 (rad)');
legend('\phi_1', '\phi_2', '\phi_3');

subplot(3, 1, 2);
plot(t, tau_hat);
% title('干扰估计');
xlabel('时间 (s)');
ylabel('估计值 (N·m)');
legend('\tau_{hat,1}', '\tau_{hat,2}', '\tau_{hat,3}');

subplot(3, 1, 3);
plot(t, gamma_hat);
% title('自适应参数');
xlabel('时间 (s)');
ylabel('参数值');
legend('\gamma_{hat}');



function dx = quadrotorDynamics(t, x, J, phi_d, phi_d_dot, phi_d_ddot, tau_d, ...
                                sigma2, cd, c_lambda, eta_d, eta_lambda)
    % 状态解码
    phi = x(1:3); % 姿态角
    phi_dot = x(4:6); % 姿态角速度
    tau_hat = x(7:9); % 干扰估计
    gamma_hat = x(10); % 自适应参数

    % 期望值
    phi_d_k = phi_d(t);
    phi_d_dot_k = phi_d_dot(t);
    phi_d_ddot_k = phi_d_ddot(t);
    tau_d_k = tau_d(t);

    % 误差计算
    phi_e = phi - phi_d_k;
    phi_e_dot = phi_dot - phi_d_dot_k;

    % 滑模变量
    s = phi_e + phi_e_dot;

    % 控制律
    tau_control = J * phi_d_ddot_k - J * tau_hat - gamma_hat * J * tanh(s) ...
                  - diag(1 ./ [sigma2, sigma2, sigma2]) * (phi_e + s);

    % 干扰估计更新
    tau_hat_dot = cd * (abs(s) - eta_d * tau_hat);

    % 自适应参数更新
    gamma_hat_dot = c_lambda * (norm(s) - eta_lambda * gamma_hat);

    % 动力学更新
    phi_ddot = J \ (tau_control - tau_d_k);

    % 状态导数
    dx = [phi_dot; phi_ddot; tau_hat_dot; gamma_hat_dot];
end
